# -*- conding:utf-8 -*-

"""
@author: PoXiao
@file: traffic_fitness.py
@self.time: 2019/3/20 0020 16:52

"""
import numpy as np
from route import Astar_new


class Fitness:
    def __init__(self, chromosome, dogcar_number, Road, Car, Cross, crossMap, vlist_table):
        self.chromosome = chromosome
        self.dogcar_number = dogcar_number
        self.Road = Road
        self.Car = Car
        self.Cross = Cross
        self.crossMap = crossMap
        #self.routeChromosome = np.zeros((self.chromosome.shape[0],self.chromosome.shape[1]), int)
        self.vlist_table = vlist_table

    def traffic_fitness(self):
        # row = self.Car.shape[0]
        # col = int(self.chromosome.shape[0] / row)
        # self.chromosome = self.chromosome.reshape(row, col)
        # allChromosome = self.chromosome.flatten()
        self.time = 0
        PositionData = np.zeros((self.Road.shape[0], max(self.Road[:,3]) * 2, max(self.Road[:, 1])), dtype=np.int)
        self.Car = np.c_[self.Car, np.zeros((self.Car.shape[0], 12))]
        self.Car = np.array(self.Car, dtype=np.int)  # 将Car里元素由float变为int
        self.Car[:, 12:17] = -1
        #self.routeChromosome = np.zeros((self.chromosome.shape[0], self.chromosome.shape[1]), int)
        while (1-((len(np.nonzero(self.Car[:, 5])[0])) == len(self.Car[:, 5]))) and self.time < 10000:
            self.time = self.time + 1
            self.Car[:, 12:17] = -1
            for i in range(0, self.Car.shape[0]):
                if (1-self.Car[i][6]) and (self.Car[i][7] and (1-self.Car[i][5])):
                    [self.Car, PositionData] = self.drive(self.crossMap, self.Car[i][0], self.Road, self.Cross, PositionData)
                    self.Car[self.find(np.where(self.Car[:, 0] == self.Car[i][0])), 11] = 0
                    self.Car[self.find(np.where(self.Car[:, 0] == self.Car[i][0])), 16] = -1
            for i in range(0, self.Car.shape[0]):
                if not self.Car[i][7]:
                    #self.Car = Astar_new(self.Car, self.Cross, self.Road, PositionData, self.chromosome[self.chromosome[:,0]==self.Car[i][0],:], self.vlist_table)
                    # cross_sequence = [0, self.find(np.where(self.routeChromosome[i, 2:] == 1)), self.find(np.where(self.routeChromosome[i, 2:] == 2))]
                    # road_next = self.crossMap[cross_sequence[1], cross_sequence[2]]
                    # if isinstance(road_next, list) == True:  # 防止road_next取到-1
                    #     road_next.remove(-1)
                    #     road_next = road_next[0]
                    # else:
                    #     road_next = road_next
                    # S2 = min(self.Road[self.find(np.where(self.Road[:,0]==road_next))][2], Car[i][3])
                    # [Car, PositionData] = self.into_road(self.crossMap, self.Road, Car, self.Cross, PositionData, Car[i][0],
                    #                                 cross_sequence, S2, self.time)
                    self.Car[self.find(np.where(self.Car[:, 0] == i)), 11] = 1
                    [road_now, road_next, direct, cross_sequence] = self.cross2road(self.crossMap, self.Road, self.Cross, self.Car[i][0],
                                                                                    PositionData)
                    self.fuck_cross_test(self.crossMap, self.Road, self.Cross, PositionData, self.Car[i][0], self.Car[i, 8:11],
                                         road_now, road_next, direct, cross_sequence)
                    # self.Car[self.find(np.where(self.Car[:, 0] == self.Car[i][0])), 16] = -1
            self.Car[:, 11] = 0
            self.Car[:, 6] = 0

        fitness = self.time
        # return fitness
        print(fitness)

    def behind_dog(self, car_number, car_position, PositionData, dogcar_global):
        if dogcar_global[2] - car_position[2] != 1:
            PositionData[dogcar_global[0]][dogcar_global[1]][dogcar_global[2] - 1] = car_number
            temp = car_position[2]
            # Car[Car[:, 0] == car_number, 8:11] = [dogcar_global[0], dogcar_global[1], dogcar_global[2] - 1]
            self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11] =\
                [dogcar_global[0], dogcar_global[1], dogcar_global[2] - 1]
            PositionData[car_position[0]][car_position[1]][temp] = 0

        self.Car[self.find(np.where(self.Car[:, 0] == car_number))][6] = 1
        return self.Car, PositionData

    def creat_crossMap(self):
        cross_size = self.Cross.shape[0]  # 获得Cross矩阵的行数
        self.crossMap = np.zeros((cross_size, cross_size))
        for i in range(0, cross_size):
            for j in range(0, cross_size):
                self.crossMap[i, j] = self.intersection(self.Cross[i, :], self.Cross[j, :])
                self.crossMap[i, i] = 0
        self.crossMap = np.array(self.crossMap, dtype=np.int)
        return self.crossMap

    def creat_roadMap(self):
        addColumn = np.zeros((self.Road.shape[0], 4))
        d = np.c_[self.Road, addColumn]
        for i, j in zip(range(self.Road[0, 0], self.Road[self.Road.shape[0] - 1, 0] + 1), range(0, self.Road.shape[0])):
            d[j, 7:9] = np.where(self.crossMap == i)[1]
            d[j, 9:11] = np.where(self.crossMap == i)[0]

        self.Road = d
        self.Road = np.array(self.Road, dtype=np.int)
        return self.Road

    def cross2road(self, crossMap, Road, Cross, car_number, PositionData):
        self.Car = Astar_new(self.Car, Cross, Road, PositionData, self.chromosome[self.chromosome[:,0] == car_number, :], self.vlist_table)
        #crossOFchromosome = self.routeChromosome[self.routeChromosome[:, 0] == car_number, 2:]
        cross_next = -1
        carRoute = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), :]
        road_now = []
        if carRoute[11] == 1:
            if carRoute[16] == -1 and carRoute[12] != -1:
                cross_next = carRoute[12]
            elif carRoute[16] == carRoute[12] and carRoute[13] != -1:
                cross_next = carRoute[13]
            elif carRoute[16] == carRoute[13] and carRoute[14] != -1:
                cross_next = carRoute[14]
            else:
                print('Next Cross Error', '2')
                cross_sequence = []
                temp = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8]
                road_now = Road[temp, 0]
                cross_now = Road[self.find(np.where(Road[:, 0] == road_now)), 7:9]
                if crossMap[cross_now[1], carRoute[16]] == 0:
                    cross_now = [cross_now[1], cross_now[0]]
                cross_sequence.extend(cross_now)
                cross_sequence.append(carRoute[16])
                road_next = crossMap[cross_sequence[1]][cross_sequence[2]]

                trackNo = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 9]
                if trackNo+1 <= Road[self.find(np.where(Road[:,0] == road_next)), 3]:
                    lane = 1
                else:
                    lane = -1
                lockFirstCar = self.find_road_firstcar(Road, road_next, PositionData, lane)
                self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 16] = -1
                self.drive(crossMap, lockFirstCar, Road, Cross, PositionData)
                self.drive(crossMap, car_number, Road, Cross, PositionData)
        else:
            cross_next = carRoute[12]
        self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 16] = cross_next

        # suckMyCock = Car[Car[:, 0] == car_number, 8][0] + 1
        # road_now1 = Road[suckMyCock][0]
        # road_now1 = Road[Car[Car[:, 0] == car_number, 8]+1][0][0]
        if self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 7]:
            road_now = Road[self.Car[self.find(np.where(self.Car[:, 0] == car_number))][8]][0]
        cross_sequence = []
        if carRoute[7]:
        # if self.Car[carRoute, 7]:
            cross_now = Road[self.find(np.where(Road[:, 0] == road_now)), 7:9]
            if crossMap[cross_now[1], cross_next] == 0:
                cross_now = [cross_now[1], cross_now[0]]
            cross_sequence.extend(cross_now)
            cross_sequence.append(cross_next)
        else:
            cross_sequence = [-1, carRoute[1]-1, cross_next]
        # if (crossOFchromosome[0, cross_now[0]]) > (crossOFchromosome[0, cross_now[1]]):
        #     cross_now = [cross_now[1], cross_now[0]]



        #idxNextCross = crossOFchromosome[0, cross_now[1]] + 1
        #cross_sequence = []
        #cross_sequence.extend(cross_now)  #将列表添加到空列表中
        #cross_sequence.append(self.find(np.where(crossOFchromosome[0, :] == idxNextCross))) #单个数值添加到列表中
        #cross_sequence.append(cross_next)
        #road_next = -1 #初始值
        print(car_number, cross_sequence)
        road_next = crossMap[cross_sequence[1]][cross_sequence[2]]
        idx_now = self.find(np.where(Cross[cross_sequence[1], :] == road_now))
        idx_next = self.find(np.where(Cross[cross_sequence[1], :] == road_next))

        direct = -1 #作为初始值
        # if (type(road_next)) == np.ndarray:
        # 当前点是终点时，终止该车，输出 direct = -1
        if cross_sequence[1] == self.Car[self.Car[:, 0] == car_number, 2]-1:
            direct = -1
            road_next = -1
        elif abs(idx_now - idx_next) == 2:
            direct = 1
        elif (idx_now - idx_next == -1) or (idx_now - idx_next == 3):
            direct = 2
        elif (idx_now - idx_next == 1) or (idx_now - idx_next == -3):
            direct = 3
        elif not idx_now:
            direct = 4
        else:
            print("Direct Error")

        return road_now, road_next, direct, cross_sequence

    def drive(self, crossMap, car_number, Road, Cross, PositionData):
        if self.Car[self.find(np.where(self.Car[:, 0] == car_number))][6] == 1:
            print("Drive Error!!!")
        self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 11] = 1
        # self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 11] = 1
        car_position = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11]
        [road_now, road_next, direct, cross_sequence] = self.cross2road(crossMap, Road, Cross, car_number, PositionData)
        R1 = Road[self.find(np.where(Road[:, 0] == road_now)), 2]
        R2 = Road[self.find(np.where(Road[:, 0] == road_next)), 2]
        V = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 3]
        V1 = min(R1, V)
        V2 = min(R2, V)
        SV1 = V1
        SV2 = V2
        S1 = Road[self.find(np.where(Road[:, 0] == road_now))][1] - (car_position[2]+1)
        S2 = max(0, SV2 - S1)
        road_rest = PositionData[car_position[0], car_position[1], (car_position[2] + 1):(min(S1, V1)+1)]
        # road_rest = road_rest.flatten()
        # road_rest = road_rest.reshape(road_rest.size)
        # road_rest = road_rest[::-1]
        # road_rest = road_rest.reshape(road_rest.shape)
        dogcar_local = np.nonzero(road_rest)[0]
        self.dogcar_number = []
        if len(dogcar_local) > 0:
            dogcar_local = dogcar_local[0] #如果dogcar_local不为空，返回第一个不为0元素的下标
            dogcar_global = car_position[0:2]
            dogcar_global = np.r_[dogcar_global, car_position[2] + dogcar_local + 1]
            self.dogcar_number = PositionData[dogcar_global[0]][dogcar_global[1]][dogcar_global[2]]
            # dog_chromosome = getcar_cho(Road, Car, Cross, chromosome, dogcar_number)

        if S1 <= V1:
            if (type(dogcar_local)) != np.ndarray:
                if self.Car[self.find(np.where(self.Car[:, 0] == self.dogcar_number))][6] == 1:
                    [Car, PositionData] = self.behind_dog(car_number, car_position, PositionData, dogcar_global)
                else:
                    [Car, PositionData] = self.drive(crossMap, self.dogcar_number, Road, Cross, PositionData)
                    [Car, PositionData] = self.drive(crossMap, car_number, Road, Cross, PositionData)
                # Car_row = self.find(np.where(self.Car[:, 0] == car_number))
                # index = int(np.argwhere(self.chromosome[Car_row, 2:] == max(self.chromosome[Car_row, 2:]))) + 2
                # self.chromosome[Car_row][index] = 0
            else:
                # if (type(road_next)) == np.ndarray:
                if road_next == -1:
                    PositionData[car_position[0]][car_position[1]][car_position[2]] = 0
                    self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11] = [-1, -1, -1]
                    self.Car[self.find(np.where(self.Car[:, 0] == car_number))][5] = 1
                elif S2:
                    [self.Car, PositionData] = self.fuck_cross_test(crossMap, Road, Cross, PositionData, car_number, car_position,
                                                                    road_now, road_next, direct, cross_sequence)
                else:
                    temp = car_position[2] + S1
                    temp2 = car_position[2]
                    PositionData[car_position[0]][car_position[1]][temp2] = 0
                    PositionData[car_position[0]][car_position[1]][temp] = car_number
                    self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11] = [car_position[0], car_position[1], temp]
                    self.Car[self.find(np.where(self.Car[:, 0] == car_number))][6] = 1
                    # 没能力过路口，清染色体
                    # Car_row = self.find(np.where(self.Car[:, 0] == car_number))
                    # index = int(np.argwhere(self.chromosome[Car_row, 2:] == max(self.chromosome[Car_row, 2:]))) + 2
                    # self.chromosome[Car_row][index] = 0

        else:
            if (type(dogcar_local)) != np.ndarray:
                if self.Car[self.find(np.where(self.Car[:, 0] == self.dogcar_number))][6] == 1:
                    [self.Car, PositionData] = self.behind_dog(car_number, car_position, PositionData, dogcar_global)
                else:
                    [self.Car, PositionData] = self.drive(crossMap, self.dogcar_number,Road, Cross, PositionData)
                    [self.Car, PositionData] = self.drive(crossMap, car_number, Road, Cross, PositionData)
            else:
                temp = car_position[2] + V1
                temp2 = car_position[2]
                PositionData[car_position[0]][car_position[1]][temp] = car_number
                self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11] = [car_position[0], car_position[1], temp]
                PositionData[car_position[0]][car_position[1]][temp2] = 0
                self.Car[self.find(np.where(self.Car[:, 0] == car_number))][6] = 1

            # 不到路口，退染色体
            # Car_row = self.find(np.where(self.Car[:, 0] == car_number))
            # index = int(np.argwhere(self.chromosome[Car_row, 2:] == max(self.chromosome[Car_row, 2:]))) + 2
            # self.chromosome[Car_row][index] = 0

        return self.Car, PositionData

    def examine_car_enableRoad(self, Road, car_number, PositionData, road_now, road_next):
        car_position = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11]          # 8:11 末尾11取不到  8  9  10
        car_d = car_position[2]
        road_now_row = self.find(np.where(Road[:, 0] == road_now))
        R1 = Road[road_now_row][2]
        nowRoadLength = Road[road_now_row][1]
        road_next_row = self.find(np.where(Road[:, 0] == road_next))
        if road_next != -1:
            R2 = Road[road_next_row][2]
        else:
            R2 = 0
        V = self.Car[self.find(np.where(self.Car[:, 0] == car_number))][3]
        V1 = min(R1, V)
        V2 = min(R2, V)
        S1 = nowRoadLength - (car_d+1)
        S2 = max(0, V2 - S1)
        if S2 > 0:
            flag = 1
        else:
            flag = -1

        return flag, S2

    def find(self, tuple):
        npwhere = tuple[0]
        if len(npwhere) > 1:
            return npwhere
        elif len(npwhere) == 1:
            return npwhere[0]
        else:
            return npwhere
    def find_road_firstcar(self, Road, Road_Number, PositionData, lane):
        car_number = -1
        roadLength = 0
        road_row = self.find(np.where(Road[:, 0] == Road_Number))
        if road_row:
            roadNumber = Road[road_row][3]
            roadBothway = Road[road_row][6]
            roadLength = Road[road_row][1]
            Road_Number = self.find(np.where(Road[:,0] == Road_Number))

        if roadLength:
            if roadBothway == 0:
                for j in range(roadLength-1, -1, -1):
                    flag = 0
                    for i in range(0, roadNumber):
                        if PositionData[Road_Number][i][j] != 0:
                            car_number = PositionData[Road_Number][i][j]
                            flag = 1
                            break
                        else:
                            car_number = -1
                    if flag == 1:
                        break
            else:
                if lane > 0:
                    for j in range(roadLength-1, -1, -1):
                        flag = 0
                        for i in range(0, roadNumber):
                            if PositionData[Road_Number][i][j] != 0:
                                car_number = PositionData[Road_Number][i][j]
                                flag = 1
                                break
                            else:
                                car_number = -1
                        if flag == 1:
                            break
                else:
                    for j in range(roadLength-1, -1, -1):
                        flag = 0
                        for i in range(roadNumber, roadNumber*2):
                            if PositionData[Road_Number][i][j] != 0:
                                car_number = PositionData[Road_Number][i][j]
                                flag = 1
                                break
                            else:
                                car_number = -1
                        if flag == 1:
                            break
        return car_number
    def fuck_cross_test(self, crossMap, Road, Cross, PositionData, car_number, CarPosition,
                        road_now, road_next, direct, cross_sequence):

        # [road_now, road_next, direct, cross_sequence] = self.cross2road(crossMap, Road, Cross, car_number, PositionData)
        S2 = -1
        if direct != 4:
            road_now_row = self.find(np.where(Road[:, 0] == road_now))
            R1 = Road[road_now_row][2]
            nowRoadLength = Road[road_now_row][1]
            road_next_row = self.find(np.where(Road[:, 0] == road_next))
            R2 = Road[road_next_row][2]

            V = self.Car[self.find(np.where(self.Car[:, 0] == car_number))][3]
            V1 = min(R1, V)
            V2 = min(R2, V)
            car_position = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11]
            car_r = car_position[0]
            car_l = car_position[1]
            car_d = car_position[2]
            S1 = nowRoadLength - (car_d+1)
            S2 = max(0, V2 - S1)
            c2 = cross_sequence[1]

        if S2 == 0:
            PositionData[car_r][car_l][car_d + S1] = car_number
            PositionData[car_r][car_l][car_d] = 0
            self.Car[self.find(np.where(self.Car[:, 0] == car_number))][6] = 1
        else:
            if direct == 1:
                [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData,
                                                          car_number, cross_sequence, S2)
            elif direct == 2:
                temp = self.find(np.where(Cross[c2, :] == road_next))
                if temp > 2:
                    temp = temp - 2
                else:
                    temp = temp + 2
                #c2_row = self.find(np.where(Cross == c2))
                c2_row = c2
                blockingRoad = Cross[c2_row][temp]
                if blockingRoad == -1:
                    [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2)
                else:
                    if (Road[self.find(np.where(Road[:, 0] == blockingRoad)), 6] == 1) or (Road[self.find(np.where(Road[:,0])==blockingRoad),6] == 0 and (Road[self.find(np.where(Road[:,0])==blockingRoad),6] == c2+1 or Road[self.find(np.where(Road[:,0])==blockingRoad),5] == c2+1)):
                        if blockingRoad < road_next:
                            lane = 1
                        else:
                            lane = -1
                        blockingCar = self.find_road_firstcar(Road, blockingRoad, PositionData, lane)
                        if blockingCar == -1:
                            [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2)
                        else:
                            blocking_car_position = self.Car[self.find(np.where(self.Car[:,0] == blockingCar)), 8:11]
                            m = blocking_car_position[0]
                            n = blocking_car_position[1]
                            p = blocking_car_position[2]
                            # blockingChromosome = getcar_cho(Road, Car, Cross, blockingCar)
                            [blocking_road_now, blocking_road_next, blocking_direct, blocking_cross_sequence] = self.cross2road(crossMap, Road, Cross, blockingCar, PositionData)
                            [enable, blockingCar_S2] = self.examine_car_enableRoad(Road, blockingCar, PositionData, blocking_road_now, blocking_road_next)
                            if blocking_direct == 1 and enable > 0 and self.Car[self.find(np.where(self.Car == PositionData[m][n][p])), 6] == 0:
                                [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, blockingCar, blocking_cross_sequence, blockingCar_S2)
                                [self.Car, PositionData] = self.fuck_cross_test(crossMap, Road, Cross, PositionData, car_number, CarPosition,
                                                                                road_now, road_next, direct, cross_sequence)
                            else:
                                [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2)
                    else:
                        # if Road[self.find(np.where(Road[:,0] == blockingRoad))][5] != c2+1:
                            [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2)
            elif direct == 3:
                temp1 = self.find(np.where(Cross[c2, :] == road_next))
                if temp1 > 2:
                    temp1 = temp1 - 2
                else:
                    temp1 = temp1 + 2
                temp2 = self.find(np.where(Cross[c2, :] == road_now))
                if temp2 > 2:
                    temp2 = temp2 - 2
                else:
                    temp2 = temp2 + 2
                #blockingRoad1 = Cross[self.find(np.where(Cross == c2))][temp1]
                blockingRoad1 = Cross[c2][temp1]
                blockingRoad2 = Cross[c2][temp2]
                #blockingRoad2 = Cross[self.find(np.where(Cross == c2))][temp2]
                if blockingRoad1 == -1:
                    blockingRoadLane1 = []
                else:
                    blockingRoadLane1 = Road[self.find(np.where(Road[:, 0] == blockingRoad1))][6]
                if blockingRoad2 == -1:
                    blockingRoadLane2 = []
                else:
                    blockingRoadLane2 = Road[self.find(np.where(Road[:, 0] == blockingRoad2))][6]
                if blockingRoad1 < road_next:
                    lane1 = 1
                else:
                    lane1 = -1
                if blockingRoad2 < road_next:
                    lane2 = 1
                else:
                    lane2 = -1
                blockingCar1 = self.find_road_firstcar(Road, blockingRoad1, PositionData, lane1)
                blockingCar2 = self.find_road_firstcar(Road, blockingRoad2, PositionData, lane2)
                direct1 = 0  #作为初始化
                direct2 = 0
                cross_sequence1 = 0 #作为初始化
                cross_sequence2 = 0
                # CarChromosome1 = 0
                # CarChromosome2 = 0
                if blockingCar1 != -1:
                    CarPosition1 = self.Car[self.find(np.where(self.Car[:, 0] == blockingCar1)), 8:11]
                    # CarChromosome1 = getcar_cho(Road, Car, Cross, blockingCar1)
                    [road_now1, road_next1, direct1, cross_sequence1] = self.cross2road(crossMap, Road, Cross, blockingCar1, PositionData)
                    [enable1, obstructionS2_1] = self.examine_car_enableRoad(Road, blockingCar1, PositionData, road_now1, road_next1)
                else:
                    enable1 = -1

                if blockingCar2 != -1:
                    CarPosition2 = self.Car[self.find(np.where(self.Car[:, 0] == blockingCar2)), 8:11]
                    # CarChromosome2 = getcar_cho(Road, Car, Cross, chromosome, blockingCar2)
                    [road_now2, road_next2, direct2, cross_sequence2] = self.cross2road(crossMap, Road, Cross, blockingCar2, PositionData)
                    [enable2, obstructionS2_2] = self.examine_car_enableRoad(Road, blockingCar2, PositionData, road_now2, road_next2)
                else:
                    enable2 = -1

                if (blockingRoad1 != -1) and (blockingCar1 != -1) and (direct1 == 1) and (enable1 == 1) and (self.Car[self.find(np.where(self.Car[:,0] == blockingCar1)),6] == 0) and (blockingRoadLane1 == 1 or (blockingRoadLane1 == 0 and (Road[self.find(np.where(Road[:,0] == blockingRoad1)),5] == c2+1) or Road[self.find(np.where(Road[:,0] == blockingRoad1)),6] == c2+1)):
                    [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, blockingCar1,
                                                    cross_sequence1, obstructionS2_1)
                    [self.Car, PositionData] = self.fuck_cross_test(crossMap, Road, Cross, PositionData,car_number, CarPosition,
                                                                    road_now, road_next, direct, cross_sequence)
                elif (blockingRoad2 != -1) and (blockingCar2 != -1) and (direct2 == 2) and (enable2 == 1) and (self.Car[self.find(np.where(self.Car[:,0] == blockingCar2)),6] == 0) and (blockingRoadLane2 == 1 or (blockingRoadLane2 == 0 and (Road[self.find(np.where(Road[:,0] == blockingRoad2)),5] == c2+1) or Road[self.find(np.where(Road[:,0] == blockingRoad2)),6] == c2+1)):
                    [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, blockingCar2,
                                                    cross_sequence2, obstructionS2_2)
                    [self.Car, PositionData] = self.fuck_cross_test(crossMap, Road, Cross, PositionData,car_number, CarPosition,
                                                                    road_now, road_next, direct, cross_sequence)
                else:
                    [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number,
                                                    cross_sequence, S2)
            elif direct == 4:
                S2 = min(self.Road[self.find(np.where(self.Road[:, 0] == road_next))][2],
                         self.Car[self.find(np.where(self.Car[:, 0] == car_number))][3])
                [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData,
                                                          car_number, cross_sequence, S2)
            else:
                print('fuck Error')

        return self.Car, PositionData

# def getcar_cho(Road, Car, Cross, chromosome ,car_number):
#     # row = int(Car.shape[0])
#     # col = int(len(allChromosome) / 8)
#     # temp = allChromosome.reshape(row, col)
#     temp = chromosome[chromosome[:,0] == car_number,:]
#     carChromosome = temp[find(np.where(temp[:,0] == car_number)),:]
#     return carChromosome

    def intersection(self, list1, list2):
        for i in range(1, 5):
            for j in range(1, 5):
                if (list2[j] == list1[i]) and (list2[j] != -1):
                    number = list2[j]
                    return number
                else:
                    number = 0

        return number

    def into_road(self, crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2):
        c1 = cross_sequence[0]
        c2 = cross_sequence[1]
        c3 = cross_sequence[2]
        # temp1 = Cross[c2, :]
        # temp2 = Cross[c3, :]
        # goalRoadNumber = intersection(temp1, temp2)  #取交集
        goalRoadNumber = crossMap[c2][c3]
        if isinstance(goalRoadNumber, list):    # 防止road_next取到-1
            goalRoadNumber.remove(-1)
            road_next = goalRoadNumber[0]
        else:
            road_next = goalRoadNumber
        road_next_row = self.find(np.where(Road[:,0] == road_next))
        goalRoadNumber = Road[road_next_row][3]  # 目标车道数目
        emptySpaceRoadNumber = -1
        S2 = int(S2)
        if c3 - c2 > 0:
            for i in range(0, goalRoadNumber):    # i 从0到goalRoadNumber-1
                flag = 0
                for j in range(0, S2):
                    if (PositionData[road_next_row][i][j]) == 0:
                        if j == (S2-1):
                            emptySpaceRoadNumber = i
                            emptySpaceRoadLength = j
                            flag = 1
                            break
                    else:
                        if self.Car[self.find(np.where(self.Car[:, 0] == PositionData[road_next_row][i][j]))][6] == 0:   #表示在道上的车没被开过
                            [self.Car, PositionData] = self.drive(crossMap, PositionData[road_next_row, i, j], Road, Cross, PositionData)
                            [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2)
                            return self.Car, PositionData
                        elif self.Car[self.find(np.where(self.Car[:, 0] == PositionData[road_next_row][i][j]))][6] == 1:
                            if j == 0:
                                break
                            else:
                                emptySpaceRoadNumber = i
                                emptySpaceRoadLength = j - 1
                                flag = 1
                                break
                if flag == 1:
                    break
        else:
            for i in range(goalRoadNumber, goalRoadNumber*2):    # i 从0到goalRoadNumber-1
                flag = 0
                for j in range(0, S2):
                    if (PositionData[road_next_row][i][j]) == 0:
                        if j == (S2-1):
                            emptySpaceRoadNumber = i
                            emptySpaceRoadLength = j
                            flag = 1
                            break
                    else:
                        if self.Car[self.find(np.where(self.Car[:, 0] == PositionData[road_next_row][i][j]))][6] == 0:   #表示在道上的车没被开过
                            [self.Car, PositionData] = self.drive(crossMap, PositionData[road_next_row][i][j], Road, Cross, PositionData)
                            [self.Car, PositionData] = self.into_road(crossMap, Road, Cross, PositionData, car_number, cross_sequence, S2)
                            return self.Car, PositionData
                        elif self.Car[self.find(np.where(self.Car[:, 0] == PositionData[road_next_row][i][j]))][6] == 1:
                            if j == 0:
                                break
                            else:
                                emptySpaceRoadNumber = i
                                emptySpaceRoadLength = j - 1
                                flag = 1
                                break
                if flag == 1:
                    break
        car_position = self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 8:11]
        car_r = car_position[0]
        car_l = car_position[1]
        car_d = car_position[2]
        Car_row = self.find(np.where(self.Car[:, 0] == car_number))
        if emptySpaceRoadNumber == -1:
            if self.Car[Car_row][7] != 0:
                roadEndPosition = Road[car_r][1]
                PositionData[car_r][car_l][car_d] = 0
                PositionData[car_r][car_l][roadEndPosition-1] = car_number
                self.Car[Car_row, 8:11] = [car_r, car_l, roadEndPosition - 1]
                self.Car[Car_row][6] = 1
        else:
            if self.Car[Car_row][7] != 0:
                PositionData[car_r][car_l][car_d] = 0
            #max_car_chromo = max(self.chromosome[Car_row, 2:])
            # if self.Car[Car_row][7]:
            #     self.chromosome[Car_row][cross_sequence[2] + 2] = max(self.chromosome[Car_row, 2:]) + 1
            #self.chromosome[self.chromosome[:, 0] == car_number, :] = self.routeChromosome[self.routeChromosome[:,0] == car_number, :]

            PositionData[road_next_row][emptySpaceRoadNumber][emptySpaceRoadLength] = car_number
            self.Car[Car_row, 8:11] = [road_next_row, emptySpaceRoadNumber, emptySpaceRoadLength]
            self.Car[Car_row][6] = 1
            if self.Car[Car_row][7] == 0:
                self.Car[Car_row][7] = self.time
                self.chromosome[Car_row][cross_sequence[1] + 2] = max(self.chromosome[Car_row, 2:]) + 1
            else:
                self.chromosome[Car_row][cross_sequence[1] + 2] = max(self.chromosome[Car_row, 2:]) + 1
        self.Car[self.find(np.where(self.Car[:, 0] == car_number)), 16] = -1


        return self.Car, PositionData
